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SUMMARY 


The computer simulation called ROBSIM, developed under the four 
phases of this contract provides the capability to perform kinematic and 
dynamic analyses of user defined, rigid link manipulators. The kinematic 
analysis provides positions, velocities, and accelerations of all parts 
of the manipulator for a prescribed motion. The dynamic analysis 
includes requirements analysis which calculates system loads for specific 
motions, and response simulation which calculates the motion resulting 
from a prescribed set of driving torques or feedback control law. This 
report documents the fourth phase of the contract which built on the 
basic capabilities developed in the phases one through three. 

The ability to utilize CAD/CAM generated data to model system 
components was added. This feature allows the user to access a file of 
CAD/CAM data written according to the Initial Graphics Exchange 
Specification, version 2.0. The ROBSIM preprocessor function can read 
this file and then write it in another format that can be read in and 
used for the detailed graphics modeling of system components. A graphics 
display is available during the reformatting procedure. 

The capability to simulate multiple arms with movable bases was also 
added for this phase of the contract. A system may be modeled with all 
of the manipulators attached to a single movable base, or each robotic 
arm may be attached to its own independent base. With this capability, 
the user defines motion trajectories for both the manipulator joints or 
end effectors and the movable bases. Position and/or force calculations 
are then carried out for the bases as well as for the joints and links of 
the arms. This addition has great utility in light of the emphasis being 
placed on telerobotics and space station and satellite servicing. 

To make the motion trajectory specification process easier, a task 
oriented motion specification module (or task command module) was added. 
This software allows the user to define a manipulator's motion by 
choosing command from a list or menu. Each command in the menu is a 
combination of some of the lower level commands currently in place in 
ROBSIM and is separated into these commands when actually implemented to 
control motion. 

The fourth addition gives the user another option for controlling a 
manipulator's motion. This option simulates a video camera mounted on 
the end-effector of an arm tracking a target. Currently, only stationary 
targets are modeled. 
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INTRODUCTION 


Background 

This report documents the results of work performed in Tasks 21 
through 24 of contract NAS1-16759, Evaluation of Automated Decisionmaking 
Methodologies and Development of an Integrated Robotic System Simulation. 
It was prepared by Martin Marietta Denver Aerospace for the NASA Langley 
Research Center in accordance with the contract statement of work. These 
tasks constitute Phase IV of an ongoing activity addressing technologies 
relevant to the design and operation of advanced manipulator systems. 

Phase I concentrated on the identifying and evaluating applicable 
artificial intelligence techniques and on developing a framework and 
mathematical models for the computer simulation. These results (Phase 1)^ 
were documented in 1982 in NASA contractor reports 165975, 165976, and 
165977. Phases II and III developed and implemented the software 
necessary to model a complete manipulator system and then perform 
kinematic and/or dynamic analyses. The results of work done under Phases 
II and II were documented in 1984 in NASA contractor reports 172401, 
172402, and 172403. 

This project is motivated by the realization that NASA advanced 
missions require the increasing use of automation technologies for both 
economic and performance reasons. Development of these complex 
technologies in a timely and cost efficient manner requires the extensive 
use of computer simulation tools that allow options to be evaluated and 
compared before building hardware prototypes. 

Contract Objectives 

The objective of this phase of the contract was to build on the 
simulation capabilities developed in Phases I through III, specifically 
by adding the following enhancements: 

1) The ability to utilize geometry data from a CAD/CAM database to 

model manipulator system components; 

\ 

2) The capability to simulate multiple arms on a single movable 
base or multiple movable bases; 

3) The addition of a task command module to make manipulator motion 
specification easier. Each task command is broken down into a 
series of lower level primitives previously used to define a 
motion; 

4) The ability to control a manipulator's motion by simulating a 
video camera mounted on the end-effector tracking a stationary 
target . 
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Report Organization 


This report consists of three volumes — the task or study results and 
two appendices. The study result volume documents the technical aspects 
of the software developed and implemented for Tasks 21 through 24 of 
contract NAS1-16759. Each task is reviewed in a separate section of the 
report, along with any impact it might have had on other parts of the 
program. Appendix A is the updated users guide, which reflects any 
changes caused by implementation of Tasks 21 through 24. Any page that 
was changed will be marked Rev A, October 1985 on the upper outside 
corner. Appendix B is the programmers guide and contains Visual Control 
Logic Representations (VCLRs) of all the subroutines as well as an 
outline of the program structure. As in Appendix A, any pages that were 
changed or added as a result of incorporation of the of the four new 
tasks are marked Rev A, October 1985. 
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TASK 21, USE OF CAD/ CAM DATA 


Completion of Task 21 required the design of an interface between ROBSIM- 
and CAD/ CAM- generated models. This interface enables the ROBSIM user to 
model the detailed graphics of a system more easily by accessing and 
using data converted from a CAD/CAM model file. This can be used for the 
detailed graphics modeling of manipulator components as well as for 
modeling the system environment, target objects, and load objects. 

The interface designed for this task reads CAD/ CAM data formatted 
according to the Initial Graphics Exchange Specification (IGES) version 
2.0. This specification (IGES) is supported by several vendors of 
CAD/ CAM systems to facilitate the exchange of information between 
different systems. IGES version 2.0 supports a number of geometric 
entities; however, for this implementation of ROBSIM the following 
entities are translated: 

1) Points; 

2) Lines; 

3) Arc Segments; 

4) Transformations. 

These entities are decomposed into individual (x, y, z) points that 
represent their polynomial curves, and are written to files that may be 
read in as obstacle entities for detailed graphics modeling during system 
definition or simply displayed on a graphics terminal. A line is defined 
by its two endpoints, whereas a circular arc segment is divided into 10 
linear segments. Transformations include both linear and rotational 
components and may be applied to any of the other geometric entities. 
Figure 1 shows a display of a manipulator generated on a Computer-Vision 
CAD/ CAM system, and converted using ROBSIM for display on an Evans and 
Sutherland graphics system. Figures 2 through 4 show the use of the 
interface software to create the detailed representations of the first 
three links of the manipulator. 

The work done for this task demonstrates the usefulness of this 
effort, and the need for work in this area to continue. However, certain 
concerns should be noted before extensive development is conducted. 

During development of the software for this task, files were 
generated on three different CAD/ CAM systems that all wrote files to the 
IGES version 2.0 specification. In all of the files the data needed to 
define the geometric entities were there, but the formats varied. This 
indicates a need for more work in the area of standardization. In 
addition, CAD/ CAM systems are still closer to automated drafting tools, 
than a tool for parts definition. Geometric features such as flanges, 
holes or fillets are not defined, only low-level entities such as arcs, 
splines, and lines are. To accomplish feature definition solids 
definition, and the definition of other intrinsic properties, further 
revisions to IGES and evolution in the product definition database format 
are needed. 
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Figure 1. - CAD/CAM-generated representation ofT3 manipulator. 
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Figure 3. - Base and link 1 modeled with CAD/CAM data. 



Figure 4 - Base, link 1 and link 2 modeled with CAD/CAM data 
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In an effort to address some of the shortcomings of the IGES 
specification, the McDonnell Aircraft Company has been working on a 
Product Definition Data Interface (PDDI) under contract to the Air Force 
Wright Aeronautical Laboratories (AFWAL). PDDI attempts to fill in some 
gaps and provide another step in the evolution to a more universal 
standard that would be an interface between all engineering, design, 
development, and manufacturing functions. Some of the milestones of this 
evolution are shown in Figure 5. More information on PDDI may be 
obtained from the ICAM CM Library, Wright-Patterson AFB, OH 45433-6533. 
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TASK 22, MULTIPLE ARMS WITH MOVING BASE 

Task 22 extended the analysis capabilities of the ROBSIM program to 
include multiple manipulator systems with moving bases. The ability to 
determine forces and moments at the base of each arm in the system is 
included with the moving base capability. This section of the report is 
divided into the following three subsections: 

1) Kinematic analysis 

2) Requirement analysis 

3) Response simulation 

Kinematic analysis 

The forward kinematic solution for a manipulator with a moving base 
is identical to that described in NASA contractor report 172401. That 
is, starting with the specified velocities and accelerations for the 
base, the velocities and accelerations of each link are obtained by 
successive application of the following recursive relations: 
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where: 


-i+l : angular velocity of link i + 1 

angular velocity of link i 

joint i + 1 relative angular velocity 

unit vector along joint i + 1 rotational axis 

translational velocity of joint i + 1 

translational velocity of joint i 

vector locating joint i + 1 in the world frame 

vector locating joint i in the world frame 

angular acceleration of link i + 1 

angular acceleration of link i 

joint i + 1 relative angular acceleration 

translational acceleration of joint i + 1 in the world 
frame 

translational acceleration of joint i in the world frame 

in the preceeding equations are expressed in terms of 
the world reference frame, and the origin of the [X-j ] frame is placed 
at joint l (fig. 6) . 

To accommodate base motion analysis, an option was added to allow 
the user to define a base time history profile. This new option is very 
similar to end-effector control of a manipulator using the time history 
profile specification. The two options available for defining base 
motion within each time segment are: 

1) Rate control; 

i) Position control. 

These options and their implementations are discussed in NASA 
contractor report 172401. The outputs generated from this option are the 
oase position, velocities and accelerations that are used as inputs to 
the recursive equations previously stated to determine the link 
velocities ana accelerations. 
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Figure 6. - 

Kinematic representation of a serial manipulator 
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Requirement analysis. 


The reaction forces and torques at the manipulator joints are 
evaluated as discussed in NASA contractor report 172401. The forces and 
torques are successively computed starting from the terminal link of the 
arm ana proceeding back to the base. Assuming the only external load 
applied to the intermediate links are the gravity forces acting through 
their centroids, the recursive equations for the joint reactions are 


f . 

— i 

t. 
— i 


£i+l + "i <Sc S i + £> 

ii+1 + ( ^i+l - V * f i+ i + iu * “i <icgi + 4> 


+ [1 . 1 a . + u. 

i i — i 


x [I ] U). 
i — i 


where : 


f.: force at joint i 

— i+l : f° rce at joint i + 1 

mass of link i 

a acceleration of centroid of link i 

-cgi 

£: acceleration of vector due to gravity 

I : inertia matrix of link i in world frame 

— ii : vector f rom origin of link i to the centroid of that link 

t_ t : reaction torque at joint i 

t i+l* reaction torque at joint i + 1 

all of the vector quantities in these equations are expressed in the 
world coordinate system. Another capability added for this task, is the 
ability to put multiple arms on one moving base (see Fig 7). In this 
case, the reaction forces and torques at the base are computed as a sum 
of all the individual base reaction forces and torques contributed by 
eacn arm that is attached to the moving base. 
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Figure 7 - Multiple arms on a single movable base. 
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A preferred method for modeling a system containing a moving base 
with multiple arms attached is to model one of the arms with the moving 
base geometric and mass properties, and the other arms with zero base 
properties. The locations of all first joints will then be referenced to 
a common base frame. 


Response simulation 


The difference between the new modification and the simulation 
discussed in NASA contractor report 172401 is the inclusion of the base 
motion in the dynamic equations of motion. These equations are solved 
for the base accelerations and the joint accelerations at each processing 
time step. To calculate the base and joint accelerations, the equations 
of motion are reformulated to add the six rigid— body base accelerations 
(three rotational, three translational). The controlling equation for an 
N— joint manipulator with a moving base is 


X - [A (0) ] _9 + b (0 , 0) 

where 


[ 1 ] 


t_ : (6 + N, 1) generalized torque vector containing 3 base 

reaction torques, 3 base reaction forces and N driving 
torques at N joints 

• • 

_0: (6 + N, 1) acceleration vector containing 3 base angular 

accelerations, 3 base translational accelerations and N 
joint accelerations 


[A(9)]: (6 + N, 6 + N) effective inertia matrix referenced to 

the base and joint coordinates, accordingly 

b(0, e): (6 + N, 1) vector of position- and velocity-related 

~ — — effective torques, including external loads, gravity, 
velocity-related inertia terms and friction 


The calculation of these terms is described in NASA contractor 
report 172401, except for the new modifications to the calculations of X 
and [A (9)] . 


Generalized torque, X - Besides the N- joint actuator torques this 
generalized torque also includes the three reaction torques and 3 
reaction forces at the base. These base reaction torques and forces can 
be generated by two methods: 

1) Read a file of base reaction torques and forces versus time; 

2) Read a base motion profile (not currently implemented). 
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The base reaction torques and forces file which can be generated 
during a requirements analysis run could be used along with the joint 
actuator torque file to drive the response simulation , thereby 
validating that the requirements analysis and response simulation. 


If a base motion is specified, part of the equations [l] will be 
solved for the N-joint accelerations using the known base motion profile 
and the N-joint torques. Then the backward recursive formulas are solved 
next for the base reaction forces and torques. 

Effective inertia matrix , [A 1 ] - As discussed in NASA contractor 
report 172401, the effective inertia matrix [A 1 ] due to the link's mass 
distribution is given by 


[A 1 ] 


= [J ] 


tM i J ! [0] 

roTTrrj 


u ] 


where 


EM I 

[I i ]: 

[A 1 ]: 

U 1 ]: 


(3, 3) diagonal matrix with link mass [M^] along the 
diagonal 

(3, 3) link i inertia matrix about the link i centroid, 
referenced to the world frame 

(6 + N, 6 + N) link i effective inertia matrix referenced to 
the base and joint coordinates 

(6, 6 + N) Jacobian matrix relating the motion of link i to 
the joints. Because the base is modeled as a 6-DOF rigid 
body, the expressions: 
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(s. 
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x (r , - R.)l 
-cgi ~2 

S. 1 
~2 


Rotating joint j < i 


and 
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.1*1 
IS I 


Sliding joint j _< i 


can be applied to the base degrees of freedom 1 _< j _< 6 with 


R. = 
~2 


r 

— cgi 

S. = 
“J 


vector locating the origin of the base frame, , 

referenced to the world system, for 1 < j < 6 

vector locating the base eg in world system, 

unit vector along base axes, referenced to the world system. 



rotating base joints 
1 < j _< 3 


sliding base joints 
4 _< j ^ 6 
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Sliding joint j < i 


where: 1 _< i _< N and 1 _< j _< 6 + N 

ji 

For j > i the components of — j are zero because a displacement at 
joint j has no effect on the absolute motion of link i. 

The total inertia matrix is calculated (as described in NASA contractor 
report 172401) using the symmetry of the inertia matrix, and a recursive 
procedure to complete the mass properties of the composite system of link 
i through N. 

In the full simulation case where a torque file is input, the equations 
of motion are then solved for the base accelerations and joint 
accelerations in terms of the state (base positions and velocities, joint 
positions and velocities), and the generalized torque i. 


- 16 - 



TASK 23, TASK COMMAND MODULE 


Task 23 added a task command module to the motion specification 
section of ROBSIM. The intent of this task was to simplify the motion 
specification process by implementing a set of task commands, in which 
each task command is a combination of some of the motion commands 
previously used. 

The commands for motion specification previously in place and 
described in NASA contractor report 172401 are now called primitives or 
primitive- level commands. The task commands added for this task are 
essentially just groupings of primitives. Both command levels are now in 
place and the user may define a motion specification file using either 
level. The primitive-level commands are grouped into two sections — 
motion and nonmotion. Table I lists these commands. 


TABLE I. - PRIMITIVE-LEVEL COMMANDS 


Motion 

Nonmotion 

joint rate control- 
joint position control 
end effector rate control 
end effector position control 

grasp a load object 
release a load object 
change tool reference point 
time delay or wait 
force/torque control on/off 
active compliance control on/off 


The task level commands added in this phase of the contract were: 

1) Pick up an object; 

2) Place an object; 

3) Normal or guarded arm movement; 

4) Hold current position; 

5) Change the end effector reference point; 

6) Use operator control; 

7) Set response simulation control mode; 

8) Sensor control of end effector position. 

When implemented, each of these commands is separated into 
primitives that are then written to a motion history file and used for 
motion control the same way as before. Sensor control of the end- 
effector position was also added to the primitive command level to keep 
consistancy between primitive and task level commands. 
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Picking up an object combines position control of the end-effector 
to move the arm to the load object and the nonmotion primitive grasp to 
combine the load object's mass properties with those of the end effector 
for subsequent motion. The position to which the end-effector moves is 
defined with the load object as the grasp point. This point is 
transformed to desired end-effector position at the end of the move by 
multiplying the grasp point by the load-to-world transformation matrix 
and adding this to the load object location vector 


Ip(tf) [P 1 ] 


. r + L. 
1-g -1 


where 


is the grasp point vector in world coordinates 

t_ is the time at the end of the move 

[P^] is the load to world transformation matrix 

is the grasp point vector in the load coordinates 

is the location of the load object in world coordinates 

The desired orientation of the end-effector at the end of the move is 
found from the approach vector and the end effector y-axis orientation 


Z = X x Y 
— e — e — e 


[T ] = [X Y Z ] 
e — e — e — e 


where 


is the load object approach vector (unit vector) in the 
world coordinate system (also a vector defining the 
direction of the end-effector x-axis in world coordinates) 

Y^ is a unit vector defining the direction of the end-effector 
y-axis in world coordinates 

Z is a unit vector defining the direction of the end-effector 
z-axis in world coordinates 


[T ] is the end-effector-to-world transformation matrix 
e 

Placing an object combines position control of the end-effector to 
move the object to a desired location and the release command to return 
the end-effector mass properties to those it had before grasping the 
object. Implementation of this option uses the algorithms currently in 
place. 


- 18 - 



The move arm task command allows the user to specify either a normal 
or guarded move. A normal move is simply position control of the end- 
effector. A guarded move is also position control of the end-effector 
but is used when in close proximity to another object to avoid 
collision. The time allowed for the move is doubled (slowing down the 
motion) and force/torque control is turned on to detect if any collisions 
occur. No sensed force would indicate that the move went smoothly and a 
sensed force or torque would mean a collision had occurred and the path 
of motion must be altered. 

The wait or time delay command holds the arm in its current position 
for a length of time specified by the user. 

Changing the end-effector reference point changes the point for 
which motion is defined when the user controls the end effector instead 
of the individual joints. 

The operator control command has not been implemented yet. 

Setting the control mode allows the user to choose a control method 
other than proportional-integral-derivative control of each joint to be 
used in the response simulation mode of ROBSIM. The control options 
currently implemented include: 

a) Hybrid force/torque control; 

b) Active compliance control; 

c) Dual arm coordinated control. 

The hybrid force/torque control and active compliance control are 
discussed in the previous ROBSIM report, NASA contractor report 172401. 
Although dual-arm control was not a requirement of this phase of the 
contract, initial work in this area was done in support of the ITA 
program. When analyzing a single manipulator arm, the ROBSIM program 
solves the dynamic equations of motion for the joint accelerations at 
each timestep and numerically integrates these accelerations using a 
fourth-order Runge-Kutta integration algorithm. The dynamic motion 
equation solved has the form 

L - [A(9)]I + b(a,|) 


where 


0 


9 ]3_ are the vectors of joint displacements, velocities, 

and accelerations 

t_ is the vector o‘f actuator drive torques and is 
calculated using a feedback control law 
is the vector of position- and velocity-related torques 

and is solved for using the recursive Newton-Euler 
equations with joint accelerations set to zero 
[A] is the instantaneous effective inertia matrix for the 

manipulator and is calculated using methods described by 
Thomas [1982] and Walker [1982]. 
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As mentioned earlier, this equation is solved for £ and a numerical 
integration algorithm obtains £ and 9. 

For this initial implementation of the dual-arm control simulation, 
an attempt was made to couple the dynamic solution equations and restrict 
the translational and rotational velocities of the end-effector to be the 
same. 

Sensor control of the end-effector position simulates a video-type 
sensor mounted on the end effector that can locate a target in the 
manipulator workspace and then move the arm toward it. 

The target is created during the system definition portion of ROBSIM 
in a manner very similar to creating a load object. The target, however, 
is drawn as four dots within the system (see Fig 8). 



Figure 8. - Manipulator system with target. 
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The addition of the task command module necessitated a slight 
modification of the load object creation/modification process. The 
process has changed minimally from that described in the previous ROBSIM 
report (NASA contractor report 172401). The same options and parameters 
still need to be defined by the user with the addition of three more 
pieces of data. This additional information requested of the user during 
the object creation process has been added in conjunction with the 
creation of a set of task level commands. 

The additional data needed for each load object are a name for the 
object, a grasp point on the object, and an approach vector for grasping 
the load object. 

The load name is a unique eight-character alphanumeric string 
identifying the load object. 

The grasp point is the x, y, z coordinates in the load local 
coordinate system that define where a manipulator end-effector must grasp 
the object to move or lift it. 

The approach vector is used to define the direction or side of the 
object that the end effector must be coming from to grasp the object. 

This vector should be defined in the load local coordinate system and 
corresponds to the direction of end-effector x-axis. 
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TASK 24, RANGING SYSTEM SIMULATION 


Task 24 added the capability to simulate a manipulator-mounted 
sensor system that provides accurate relative position and orientation 
determination between the manipulator end-effector and a specified 
target. This information is then used to control the motion of the 
manipulator. The algorithm implemented is based on a paper by Robert M. 
Haralick [Haralick, 1980]. This algorithm calculates the camera (sensor) 
pointing angles and distances to a target based on the perspective 
projection of the four corner dots of the target. 

Given an x-y-z coordinate frame, the perspective projection of the 
point (xq, y Q , z Q ) is defined in Figure 9. 

Note that by convention the y-axis is along the line of sight (LOS) 
of the sensor and the image plane. Where starred coordinates (X*, Z*) 
exist, the LOS is placed one focal length, f, in front of the camera 
lens, which is at the origin. 


z 



Figure 9. - Definition of the perspective projection. 
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The camera pointing angles (note that on the camera line of sight 
points in type positive y-axis direction) are defined as: 

Counterclockwise rotation about the z-axis. (theta). 

-90° < 0 £ 90° 

Counterclockwise rotation about the x-axis (phi). 

-90° <<{><_ 90° 

Clockwise rotation about the y-axis (xi). 

-180° < E, < 180° 


Given a reference frame to start from, a sequence of pointing 
angles can arbitrarily orient a sensor (Fig. 10). 

A direction cosine matrix (sometimes called the "attitude" matrix) 
is a 3x3 matrix that provides the mathematical connection between the 
coordinates of a given point in the reference frame and the coordinates 
of the same point in a rotated frame. 



Figure 10. - 

Sensor coordinate frame rotated 
relative to the reference coordinate 
frame. 
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The entries in the 3x3 matrix [A] depend on the 0 4> 5 angles 

o£ the particular rotation. 


For a counterclockwise rotation 0 about the z-axis, the direction 
cosine matrix is given by 


< A z> 


cos0 sin0 

-sin0 cos0 

0 0 


0 

0 

1 


For a counterclockwise rotation <j> about the x-axis, the direction 
cosine matrix is given by 


[A ] 
x 


1 

0 

0 


0 

cos<j> 

~sin<t> 


0 

sin<j> 

cos<j> 


For a clockwise rotation E, 
cosine matrix is given by 


[A ] = 

y 


cosC 

0 

-sin 5 


about the y-axis, the direction 

0 sin5 

1 0 

0 cosC 


Now, given an arbitrary orientation, the direction cosine matrix is 
obtained by multiplying the sequence of matrices' for 0 <j> £ with 
the 0 matrix on the right and the 5 matrix on the left. Therefore 


[A] = [A y ] [A x ] [A z ] 


cos5cos0 + sin£sin<j>sin0 
-cos<j>sin0 

-sin£cos0 + cos£sin4>sin0 


cos5sin6 - sin5sin<f>cos0 
COS(f)COS0 

-sin£sin0 - cos5sin<j>cos0 


sin£cos<j> 

sinq> 

cos£cos<p 


To calculate the perspective projection in a rotated sensor frame of 
a point P given reference frame coordinates ( x 0 , y 0 , z Q ), we first 
convert to sensor frame coordinates. Given the sensor pointing 
angles 0 <f> £ we have 


[A]P = P 
—ref — sen 


sen 
( V sen 
(2 o } sen 
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Now applying the definition of perspective projection, the 
perspective projection of P in the sensor frame is given by 


x * = f ( x °> sen 
Cy o ) sen 


2 * a f SeI1 

(yo) sen 


and .substituting, we get 


^ xo(cosgcos0 + singsin4>sin8) + yo(cosgsin9 - singsin<)>cos6) + z o (sinScosift) 

xo (-costj)sin0) + yo (cos<f>cos9) + zo(sin<j>) 

f xo(-singcos8 + cosgsimftsinQ) + yo(-singsln6 - cosgsin^cosQ) + 2q(cos£cos6) 

xq (-cos<j>sin9) + yo (coscj>cos0) + z 0 (sin<}>) 


Furthermore, these equations can be neatly expressed as 


X* 


cos£ 

sinC 

z* 


-sin? 

cos£ 


^ x 0 cos9 + y 0 sin9 

x Q (-cos<?sin6) + y o (costpcoso) + zo(sin<?) 


f 


xpsinosinS - ypsinfocosS + zqcos 4 > 

xo (— cos<psin0) + y 0 (cos<pcos0) + z 0 (sin<}!) 
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This result is obtained by factoring out cos C and sin 5 from the 
first equation for x* and z*. The advantage to these expressions is that 
the dependence on 5 has been neatly separated from 0 and 4> . 

Now we attempt to go in the reverse direction. That is, assuming 
knowledge of the perspective projection, what can one say about the 
possible points in three-dimensional space that produced it? The set of 
points in three-dimensional space that could produce the perspective 
projection (x*, z*) are given by the ray of reference frame coordinate 
defined by 


Hcl 


x^cosS - fsin0cos4> + z '*sin0sin<j> 

1 

= X 

x^sin© + fcos0cosd> - z "cos6sin<j> 
fsin<j> + z'cosiji 


for some X and where 




-sinS 

cosC 



This result comes from the observation that any point on the ray has 
coordinates (x*, f, z*) in the sensor frame. To express this in the 
reference frame we multiply by [A] -1 


X 

-1 

X* 

y 

z 

- — 

- x [ A ] 

f 

z* 




”cos 9 

-sin0cos<f) 

sin0sin(f> 


“ JU 

x"cos E, - 

z*sin? 

= 

X 

sin6 

cosdcos<p 

-sintpcosd 


f 




0 

sin<j> 

cos<p 


x*sin£ + 

z*cos? 


Note that [A] -1 is obtained by doing the negative of the original 
rotation angles in the reverse sequence. 

The perspective projection of a target can then be accomplished by 
finding the projection of each of the four corners. 

Similarly, the reverse calculations may also be^carried out (i.e., 
given the perspective projections of the corners Pj_“ p 2 " p 3 " p^“ 

find the pointing angles 0 <b and Z, ) (see Fig. 11 and 12 ). 
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Figure 11. - 

Toe coordinates of the comers of the target 
rectangle in the reference frame parallel to the 
rectangle sides . 





Establish the notation 



We can apply the results of the previous section to each of the 
corners. For K=l, 2, 3, 4 



where 

> x cos0 + y sinQ 

x = f < 

K 

x (-coscfisine) + y ( cos4>cos0 ) + z (sin<j>) 

1C 1C K 

„ c »-sin<j)sin9 - y sin<f>cos0 + z cos<l> 
z ° f k < < 


x^_ (-costj>sin0) + y K (cos4>cos0) + z^(sin<f>) 

and going in the reverse direction we know that there are four constants 
Ai A 2 A 3 Ai, such that if 


"x “ 


cos? 

-sin? 

z* 

K 


sin? 

cos? 



— 

- 



and 


X 

K 

y K 

V 

r< 

II 

^ * + ~ 

x^_ cos0 - fsin0cos<j> + z^ sin0sin<j> 

* + 

x sin0 + fcos0cos<f> - z cos0sind> 

Z 

K 


fsind) + z cos d> 

_ k _ 


then (x, y, z) are the reference coordinates of a point on the ray 
emanating from the origin through the point P* . This^gives us a 
starting point to go from the perspective projections Pi” p^' P 3 * Pi*'' 
to the sensor pointing angles 0 <j> E, . Now we use the fact that these 

perspective projections are of a rectangle. 
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xi + 

yi 

Zl + 



[ 1 ] 


XI 

yi 

21 


Xi 



'cos 0 - fsin9cos<i> + zi 
"sin0 + fcos0cos<}> - zi 
fsin<|> + zi'cos* 


*sin8sin<?> 

•*cos6sin<f> 


xi + w 

yi 

Zi 


X 2 


"x 2 > cos0 - f sin8cos<j> + z 2 
X2 'sin0 + fcosecostj) - 
fsin<j> + zz'cos* 


sin6sin4> 

cosQsim}) 


[2] 


xi 

yi 

Zj, + L 


X 3 


* x - 'cos0 - f sin8cos4> + Z3'’sin0sin<t) 
Xa^sinQ + f cosQc.os<j> - za'cosBsini}) 
fsiruji + z_3*cos$ 


[3] 


xi + w 

yi 

Zl + L 


U 


- X4 'cos0 - f sin0cos<j> + Zi^sinOsimj) 
x^sinQ + fcos0cos4> - Z4 "cosBsinij) 
f sin<j> +■ zi*'cos<j> 


[>] 
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These equations give enough information to solve for 8 4> € 

without knowing L, W, x^, y^» z^, Xx» X2» ^3» To begin, notice that 
the first and second coordinates of equations [l] and [3] are the same 
(xj and yp 

Xi(xx"*cos0 - f sin0cos<£ + zx^sin9sin<j>) 

= XsCxa^cosQ - fsin9cos<j> + z 3"sin9sin(j>) 

Xx(xi^sin0 + fcos0cos<j> - zx‘*cos8sin<{)) 

= X 3 (x3''sin0 + fcos0cosff> - z 3 ■*cos0sin<j>) 

Cross multiplying, canceling Xx*X 3 and combining like terms 

(xi** - xsOfcosi}) + (x 3 'zi' - xi J ’z 3 '')sin!j) = 0 

(x 3 " - xiO 

tancj> = f 

(x 3 'z 1 / - xx'zs') 

Now, notice that the first and second coordinates of equations [2] 
and [4] are the same. Apply the same steps as for [l] and [3] and get 

(X2 * - X4"*)fCOScj> + (xi) ^Z 2 * — X2 - *Z4 - ')sin^ = 0 


This gives an alternate expression for tan <p 


tan<J> 


f (X4" - x 2 ‘*) 
(xi t 'z 2 ' - 


Thus <i> can be calculated two ways 


<j> = tan 


-1 





f CX3" ~ Xx") 

-1 

= tan 

f CX4 * ~ X 2 ') 

X 3 Zl - Xx Z 3 

(x 4 'z 2 ' - X 2 ‘°z 4 -’) 

_ — 


— 


[5] 


This gives us a way to calculate 4> once the primed coordinates are 
known. Now to solve for 0 , notice that the second and third coordinates 
of [l]and [2] are the same (y* and z 1 ) 

XxCxi'sin© + fcos8cos4> - zx'’cos9sin<j>) 

= X 2 (x2'*sin0 + fcos9cos4> -z 2 '*cos0sinb) 


Xx(fsin<j> + zx - cos<j>) * X 2 (fsin<j) + za^cos 6) 

Cross multiplying, cancel Xx*X 2 and combine like terms 

(xx** - x 2 f s in 9 sin 0 + (xx*z 2 < - x 2 '*Zx'*) sin9cos<j> + (zx* - zx'')fcos9 
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and by symmetry (the second and third coordinates of [3] and [4] are the 
same: y^ and zj + L) 

(x 3 ' - x 4 ')fsinesincf> + (x 3 'z k ' - x 4 -*z 3 ') sin0cos<f> + (z 4 ' - z 3 '’)fcos0 = 0 

Take these, two equations and eliminate the first term by multiplying 
the first one by (x 3 ** - x 4 "*) and the second one by(xx' - x 2 ‘‘)and then 
subtracting 

C( x 3"* ~ x 4 "*) (xi'*Z 2 ‘* - x 2 ”*z i **) - (x/ - x 2 (x 3 'zi t ‘' - x 4 "z 3 ") ] sinOcosp 

+ [ (x 3 - - x 4 ') (z 2 - - Zl ') - ( Xl ' - x 2 ') (z 4 * - z 3 ')] fcose = 0 


(XX s * - x 2 ') (z 4 ' - x 3 -) - (x 3 ' - x 4 ') (z 2 ' - zx') 

tan0 = f. ■ — 

[ (x 3 * - X 4 ') ( Xl 'z 2 ' - X2 *Zx') - (xx' - X 2 ") - x 4 'z 3 , ')]cos6 

This gives a way to calculate 0 once <t> and the primed coordinates 
are known. 

Now lets go back to equation [5]. Because these are both equal to 

tan <)> 

( x i * - x 3^)f = (x a * - X 4 Qf 

xi^za** - x 3 '’zx'' x 2 'z^' - x^z 2 ' 


From this and the definition of the primed coordinates we have 
(xi*cosC - zx*sinS) - (x 3 *cos 5 - z 3 *sin?) 

_ _ _ X X 

Xx“ z 3 " - X 3 -Zx" 

(x 2 "C o s5 - z 2 *sinO - (x 4 *cos? - z 4 *sinC) 
x 2 * z 4 * - x 4 * z 2 * 

Note that the denominators result from the rotational ly invariant 
xx*z 3 * - x 3 *z x * = xx'z^ - x 3 'zx" etc. 

( x i" — X3 ' n )cos 5 + (z 3 “ — zi") sin? _ (x 2 * — x 4 *)cos 5 + (z 4 * — Z 2 *) sinC 

X1*Z 3 * - X 3 *Z X ' f X 2 ” Z 4 * - X 4 " Z 2 “ 


tan£ 


Divide both sides by cos 5 and solve for tan 5 
(xx*-x 3 *) (x 2 *z 4 * - x 4 *z 2 *) - (x 2 * - x 4 *) (xx*z 3 * - X 3 ''Z x “) 
(z 4 * - Z 2 *) (xi*Z 3 * - X 3 “Z x *) - (Z 3 * - Zl*) (x2 "Z 4 * - X 4 *Z 2 *) 
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_ . * * * * 

Now we have a complete procedure for starting with Pi p 2 P3 Pt* 
and arriving at 6 <p 5 
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CONCLUDING REMARKS 


This report has documented the work done in Phase IV of contract 
NAS7-16759 Evaluation of Automated Decisionmaking Methodologies and 
Development of an Integrated Robotic System Simulation. The tasks (21 
through 24) added for this phase extend the capabilities and versatility 
of the ROBSIM program as follows: 

1) Access to CAD/CAM data - This software module addressed task 21 
of the contract. It allows the user to access and display 
CAD/ CAM generated data that is written to a file in accordance 
with the Initial Graphics Exchange Specification (IGES) version 
2.0. The user may also use this data for the detailed graphics 
modeling of system components. 

2) Multiple arms with movable bases - This work addressed task 22 
of the contract. The user may now model and do analyses on 
systems that include up to five manipulator arms. In addition, 
the arm bases may have motion specified for them. Each arm aray 
be mounted on an independent movable base, or a single base may 
be attached to more than one arm. 

3) Task command module - This addresses task 23 of the contract and 
was added to make the procedure for motion trajectory 
specification of a manipulator easier. Each of the task 
commands is a combination of one or more of the lower level 
motion specification commands currently in place. 

4) Ranging system simulation - This addresses task 24 of the 
contract. This gives the user another way of controlling the 
robot motion. The software developed simulates a video camera 
mounted on the end-effector of a manipulator. The camera looks 
for a user specified target and moves the end-effector towards 
it. 

The ROBSIM program is a useful tool for the analysis and design of 
manipulator system components. There are, however, areas for future 
enhancement than would make the program even more versatile and useful. 
The areas for expansion could be directed toward the support of 
teleoperator systems and other robotic systems capable of remote space 
operations such as satellite servicing or space station construction. 

The addition of hand controllers to drive the simulation would have 
great utility in the area of telerobotics and teleoperator control. The 
system would be useful for manipulator operator training and for use as a 
predictive display in servicing operations having large time delays. 

Interfacing the analysis software to multiple graphics systems to 
try and make it somewhat device independent would increase interest in 
using the program. Other interfaces of interest would be to existing 
software packages in various areas like controls analysis, finite element 
analysis, and flexibility. 
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Work in the area of trajectory planning should be continued. Also, 
the hardware/ software validation efforts should be carried on to better 
define the strong points of the simulation and the areas requiring more 
work. In order to validate the software running on different systems, a 
library of test cases should be built and documented. Another useful 
library would be one containing validated models of several popular 
manipulators. 

Lastly, the developments needed to implement real-time control could 
be investigated. 
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